char incomingByte = '\0';

#define HEAD 0xFF //x3
#define MASK 0xF0 //Last byte (4°) is a mask
#define FOOTER 0xFF //Footer to count


void readSerial1(void)
{
  //int incomingByte = 0;	// for incoming serial data
  s1blen = 0;
  while (Serial1.available()>0 && s1blen < MAXBUFF){
    //Serial.println("S");
    s1buff[s1blen] = Serial1.read();
    //incomingByte = s1buff[s1blen-1];
    // say what you got:
    //Serial.print("INC:");
    //Serial.write(s1buff+s1blen,1);
    //Serial.println(incomingByte, DEC);
    //s1buff[s1blen-1] = icomingByte;
    //Serial.println("E");
    s1blen++;
  }
  
  Serial.write(s1buff,s1blen);

  /*
  boolean completed = false;
  for (int i=0; i<s1blen;i++){
    if (!s1start){
      //Search for header
      if ( s1buff[i] == HEAD && ( !s1parsing || (s1parsing && s1statecount >0) ) ){
        s1statebuf[s1statecount] = s1buff[i];
        s1statecount++;
        s1parsing = true;
        //Serial.println("START!");
      }else if (s1buff[i] & MASK == MASK && (s1parsing && s1statecount >= 3) ){
        //Leggo la maschera, riconosco il size e attivo il dump del frame
        s1statebuf[s1statecount] = s1buff[i];
        s1statecount++;
        
        s1mode = s1buff[i] & ~MASK;
        Serial.print("MODE:");
        Serial.print(s1mode);
        Serial.println();
        
        //Resetto il parsing
        s1statecount = 0;
        s1parsing = false;

        s1ftmplen = 0;
        s1start = true;
        
        for (int j = 0; j<s1statecount;j++){
          s1ftmpbuf[s1ftmplen] = s1statebuf[j];
          s1ftmplen++;
        }
        
      }else if (s1parsing){
          //Resetto il parsing
          s1statecount = 0;
          s1parsing = false;
      }
    }
    if (s1start){
      //Search for terminator
      s1ftmpbuf[s1ftmplen] = s1buff[i];
      s1ftmplen++;
      if (s1buff[i] == FOOTER){
        s1footercount++;
        if (s1footercount == s1mode){
          s1start = false;
          s1frame = true;
          s1foutbuf = s1ftmpbuf;
          s1foutlen = s1ftmplen;
          //Serial.println("END!");
          completed = true;
          s1footercount == 0;
        }
      }else{
        s1footercount = 0;
      }
    }
    if (completed){
      break;
    }
  }
  */
}

void readSerial2(void)
{
  s2bsta = s2blen;
  //s2blen = 0;
  while (Serial2.available()>0){
    uint8_t inb = Serial2.read();
    s2buff[s2blen] = inb;
    s2blen++;
    if (s2blen >= MAXBUFF){
      s2blen = 0;
    }
    int thr = s2blen+1;
    if (thr >= MAXBUFF){
      thr = 0;
    }
    if (thr == s2bsta){
      break;
    }
    //Serial.print(inb,HEX);
    //Serial.print(" - ");
    //Serial.print(s2buff[s2blen-1],HEX);
    //Serial.println();
    //Serial.write(s2buff[s1blen-1],1);
  }
  
  
  //Parsing
  boolean completed = false;
  int i = s2bsta;
  while (i != s2blen){
    //Serial.print(s2buff[i],HEX);
    //Serial.print(" - ");
    if (!s2start){
      //Search for header
      if ( s2buff[i] == HEAD && ( !s2parsing || (s2parsing && s2statecount >0) ) ){
        s2statebuf[s2statecount] = s2buff[i];
        s2statecount++;
        s2parsing = true;
        if (s2statecount >3){
          s2statecount = 3;
        }
        //Serial.println(s2statecount);
      }else if ((s2buff[i] & MASK) == MASK && (s2parsing && s2statecount == 3) ){
        //Leggo la maschera, riconosco il size e attivo il dump del frame
        s2statebuf[s2statecount] = s2buff[i];
        s2statecount = 4; //Stato 4 è il normale
        
        s2mode = s2buff[i] & ~MASK;
        //Serial.print("MODE:");
        //Serial.print(s2mode);
        //Serial.println();
        
        //Resetto il parsing
        s2parsing = false;

        s2ftmplen = 0;
        s2start = true;
        
        for (int j = 0; j<s2statecount;j++){
          s2ftmpbuf[s1ftmplen] = s2statebuf[j];
          s2ftmplen++;
        }
        
      }else{
          //Resetto il parsing
          s2statecount = 0;
          s2parsing = false;
          //Serial.println(s2statecount);
      }

    }else{
      //Search for terminator
      s2ftmpbuf[s2ftmplen] = s2buff[i];
      s2ftmplen++;
      if (s2buff[i] == FOOTER){
        s2footercount++;
        if (s2footercount == s2mode){
          s2start = false;
          s2frame = true;
          s2foutbuf = s2ftmpbuf;
          s2foutlen = s2ftmplen;
          //Serial.println("END!");
          completed = true;
          s2footercount == 0;
        }
      }else{
        s2footercount = 0;
      }
    }
    if (completed){
      //break;
    }
    
    //Incremento il mio indice
    i++;
    if (i >= MAXBUFF){
      i = 0;
    }
//    s2bsta = i;
  } //END WHILE
  
  //Serial.write(s2buff,s2blen);
  //Serial.println();

}

void readSerial3(void)
{
  s3blen = 0;
  while (Serial3.available()>0){
    s3buff[s3blen] = Serial3.read();
    s3blen++;
  }

  Serial.write(s3buff,s3blen);

}


int Parse2Bytes(uint8_t* buff, int idx){
  int retval = 0;
  retval = s2foutbuf[idx];
  retval = retval <<8;
  retval += s2foutbuf[idx+1];
  return retval;
}


